#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Task to Mission JSON Converter
将任务数据转换为PX4任务格式的JSON
"""

import json
from typing import Dict, List, Any, Optional
from datetime import datetime


def build_mission_plan(speed: float, takeoff_height: float, yaw_mode: int, 
                     yaw_angle: float, path: List[List[float]], is_gps: bool, 
                     end_z: float = 0.8) -> List[Dict[str, Any]]:
    """
    构建任务计划列表
    
    Args:
        speed: 飞行速度
        takeoff_height: 起飞高度
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度
        path: 路径点列表，格式为[[x1,y1,z1], [x2,y2,z2], ...]
        is_gps: 是否为GPS模式
        end_z: 结束高度
    
    Returns:
        任务计划列表
    """
    plan = []
    
    # 添加起飞动作
    plan.append({
        "behavior": "takeoff",
        "method": "__call__",
        "args": {
            "height": takeoff_height,
            "speed": speed,
            "wait": True
        }
    })
    
    # 添加路径跟随动作
    if is_gps:
        plan.append({
            "behavior": "follow_path_gps",
            "method": "__call__",
            "args": {
                "geopath": path,
                "speed": speed,
                "yaw_mode": yaw_mode,
                "yaw_angle": yaw_angle,
                "wait": True
            }
        })
    else:
        plan.append({
            "behavior": "follow_path",
            "method": "__call__",
            "args": {
                "path": path,
                "speed": speed,
                "yaw_mode": yaw_mode,
                "yaw_angle": yaw_angle,
                "wait": True
            }
        })
    
    # 添加返回原点动作
    plan.append({
        "behavior": "go_to",
        "method": "__call__",
        "args": {
            "x": 0.0,
            "y": 0.0,
            "z": end_z,
            "speed": 1.0,
            "yaw_mode": 0,
            "yaw_angle": 0.0,
            "wait": True
        }
    })
    
    return plan


def build_mission_object(target: str, plan: List[Dict[str, Any]]) -> Dict[str, Any]:
    """
    构建完整的任务对象
    
    Args:
        target: 目标无人机标识
        plan: 任务计划列表
    
    Returns:
        任务对象
    """
    return {
        "target": target,
        "plan": plan
    }


def make_load_payload(drone_id: str, mission_id: int, mission_obj: Dict[str, Any]) -> Dict[str, Any]:
    """
    创建加载任务的载荷
    
    Args:
        drone_id: 无人机ID
        mission_id: 任务ID
        mission_obj: 任务对象
    
    Returns:
        加载任务的载荷
    """
    return {
        "drone_id": drone_id,
        "mission_id": mission_id,
        "action": 1,  # 1表示加载任务
        "mission": json.dumps(mission_obj)
    }


def make_start_payload(drone_id: str, mission_id: int) -> Dict[str, Any]:
    """
    创建启动任务的载荷
    
    Args:
        drone_id: 无人机ID
        mission_id: 任务ID
    
    Returns:
        启动任务的载荷
    """
    return {
        "drone_id": drone_id,
        "mission_id": mission_id,
        "action": 2  # 2表示启动任务
    }


def convert_waypoints_to_path(waypoints: List[Dict[str, float]], is_gps: bool = True) -> List[List[float]]:
    """
    将航点列表转换为路径点格式
    
    Args:
        waypoints: 航点列表，格式为[{"lat": 34.3442, "lng": 108.9074, "alt": 100}, ...]
        is_gps: 是否为GPS模式
    
    Returns:
        路径点列表，格式为[[x1,y1,z1], [x2,y2,z2], ...]
    """
    path = []
    for wp in waypoints:
        if is_gps:
            # GPS模式：[lat, lng, alt]
            path.append([wp["lat"], wp["lng"], wp["alt"]])
        else:
            # 地图模式：如果有x,y,z则使用，否则使用lat,lng,alt
            x = wp.get("x", wp.get("lat", 0.0))
            y = wp.get("y", wp.get("lng", 0.0))
            z = wp.get("z", wp.get("alt", 0.0))
            path.append([x, y, z])
    return path


def convert_task_to_mission(task_data: Dict[str, Any], drone_id: str, mission_id: int = 5001, 
                           speed: float = 3.0, takeoff_height: float = 10.0, 
                           yaw_mode: int = 0, yaw_angle: float = 0.0, 
                           end_z: float = 0.8, is_gps: bool = True) -> Dict[str, Dict[str, Any]]:
    """
    将Task数据转换为mission JSON格式
    
    Args:
        task_data: 任务数据，包含航线信息
        drone_id: 无人机ID
        mission_id: 任务ID
        speed: 飞行速度
        takeoff_height: 起飞高度
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度
        end_z: 结束高度
        is_gps: 是否为GPS模式
    
    Returns:
        包含LOAD和START载荷的字典
    """
    # 获取航线坐标信息
    path_points = []
    
    # 优先从route_coordinates获取
    if task_data.get("route_coordinates"):
        path_points = convert_waypoints_to_path(task_data["route_coordinates"], is_gps)
    
    # 其次从task_routes获取
    elif task_data.get("task_routes"):
        # 按order_index排序航线
        sorted_routes = sorted(task_data["task_routes"], key=lambda x: x.get("order_index", 0))
        
        for task_route in sorted_routes:
            if task_route.get("route") and task_route["route"].get("waypoints"):
                route_points = convert_waypoints_to_path(task_route["route"]["waypoints"], is_gps)
                path_points.extend(route_points)
    
    # 如果还是没有路径点，抛出异常
    if not path_points:
        raise ValueError("任务中未找到有效的航线坐标信息")
    
    # 构建任务计划
    plan = build_mission_plan(speed, takeoff_height, yaw_mode, yaw_angle, path_points, is_gps, end_z)
    
    # 构建任务对象
    mission_obj = build_mission_object(drone_id, plan)
    
    # 创建载荷
    load_payload = make_load_payload(drone_id, mission_id, mission_obj)
    start_payload = make_start_payload(drone_id, mission_id)
    
    return {
        "load": load_payload,
        "start": start_payload,
        "mission_object": mission_obj
    }


def parse_waypoints_string(waypoints_str: str, expect_len: int = 3) -> List[List[float]]:
    """
    解析字符串格式的路径点列表
    
    Args:
        waypoints_str: 路径点字符串，每行一个点，逗号分隔
        expect_len: 期望的坐标维度
    
    Returns:
        路径点列表
    """
    out = []
    if not waypoints_str:
        return out
    
    for line in waypoints_str.strip().split('\n'):
        line = line.strip()
        if not line:
            continue
        
        parts = line.split(',')
        if len(parts) >= expect_len:
            nums = []
            valid = True
            for i in range(expect_len):
                try:
                    num = float(parts[i].strip())
                    nums.append(num)
                except ValueError:
                    valid = False
                    break
            
            if valid:
                out.append(nums)
    
    return out


def build_zenoh_url(base_url: str, key_prefix: str, drone_id: str, mission_id: int) -> str:
    """
    构建Zenoh REST URL
    
    Args:
        base_url: REST基础URL
        key_prefix: Zenoh键前缀
        drone_id: 无人机ID
        mission_id: 任务ID
    
    Returns:
        Zenoh REST URL
    """
    clean_base = base_url.rstrip('/')
    key = f"{key_prefix}/{drone_id}/mission_update"
    return f"{clean_base}/{key}"


if __name__ == "__main__":
    # 示例用法
    sample_task = {
        "id": 1,
        "name": "城市巡逻任务",
        "task_routes": [
            {
                "order_index": 0,
                "route": {
                    "waypoints": [
                        {"lat": 34.3442, "lng": 108.9074, "alt": 100},
                        {"lat": 34.3542, "lng": 108.9174, "alt": 100},
                        {"lat": 34.3542, "lng": 108.9274, "alt": 100},
                        {"lat": 34.3442, "lng": 108.9274, "alt": 100},
                        {"lat": 34.3442, "lng": 108.9074, "alt": 100}
                    ]
                }
            }
        ]
    }
    
    # 转换为mission格式
    result = convert_task_to_mission(
        task_data=sample_task,
        drone_id="uav_1",
        mission_id=5001,
        speed=3.0,
        takeoff_height=10.0,
        is_gps=True
    )
    
    # 打印结果
    print("\n=== Mission LOAD Payload ===")
    print(json.dumps(result["load"], indent=2, ensure_ascii=False))
    
    print("\n=== Mission START Payload ===")
    print(json.dumps(result["start"], indent=2, ensure_ascii=False))
    
    print("\n=== Mission Object ===")
    print(json.dumps(result["mission_object"], indent=2, ensure_ascii=False))